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Abstract 

In  this  paper  we  present  an  observability-based  local  path  planning  and  col¬ 
lision  avoidance  technique.  Bearing-only  measurements  are  utilized  to  esti¬ 
mate  the  time-to-collision  (TTC)  and  bearing  to  obstacles  using  an  extended 
Kalman  Filter  (EKF).  To  ensure  the  error  covariance  matrix  computed  by 
an  EKF  is  bounded,  the  system  should  be  observable.  We  perform  a  non¬ 
linear  observability  analysis  to  obtain  the  necessary  conditions  for  complete 
observability.  We  use  these  conditions  to  design  a  path  planning  algorithm 
that  enhances  observability  while  avoiding  collisions  with  obstacles.  We  an¬ 
alyze  the  behavior  of  the  path  planning  algorithm  and  specially  define  the 
environments  where  the  path  planning  algorithm  will  guarantee  collision-free 
paths  that  lead  to  a  goal  configuration.  Numerical  results  show  that  the  plan¬ 
ning  algorithm  successfully  solves  the  single  and  multiple  obstacle  avoidance 
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1.  Introduction 

Small  and  Miniature  Air  Vehicles  (MAVs)  have  the  potential  to  perform 
tasks  that  are  too  difficult  or  dangerous  for  human  pilots.  For  example,  they 
can  monitor  critical  infrastructure  and  real-time  disasters,  perform  search 
and  rescue,  and  measure  weather  in-storms  [1].  For  many  of  these  applica¬ 
tions,  MAVs  are  required  to  navigate  in  urban  or  unknown  terrain  where 
obstacles  of  various  types  and  sizes  may  hinder  the  success  of  the  mission. 
MAVs  must  have  the  capability  to  autonomously  plan  paths  that  do  not  col¬ 
lide  with  buildings,  trees  or  other  obstacles.  Therefore,  the  path  planning 
and  obstacle  avoidance  problems  for  MAVs  have  received  significant  atten¬ 
tion  [1,  2,  3,  4,  5]. 

The  path  planning  problem  can  be  grouped  into  global  path  planning 
and  local  path  planning.  Global  path  planning  requires  complete  knowledge 
about  the  environment  and  a  static  terrain.  In  that  setting  a  collision-free 
path  from  the  start  to  the  destination  configuration  is  generated  before  the 
vehicle  starts  its  motion  [6].  The  global  path  planning  problem  has  been 
addressed  by  many  researchers  with  common  solutions  being  potential  fields 
methods,  roadmap  methods  and  cell  decomposition  methods  [7].  On  the 
other  hand,  local  path  planning  is  executed  in  real-time  during  flight.  The 
basic  idea  is  to  first  sense  the  obstacles  in  the  environment  and  then  determine 
a  collision-free  path  [1] . 
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Local  path  planning  algorithms  require  sensors  to  detect  obstacles.  Among 
the  suite  of  possible  sensors,  a  video  camera  is  cheap  and  lightweight  and  fits 
the  physical  requirements  for  small  UAVs  [1],  However,  because  of  projective 
geometry,  a  monocular  camera  really  only  measures  the  bearing  to  the  object. 
TTC  can  be  estimated  by  considering  the  change  in  the  size  of  the  object  in 
the  image  plane,  but  this  estimate  relies  on  accurately  segmenting  the  image, 
which  can  be  a  noisy  process.  Therefore,  it  is  a  reasonable  engineering  choice 
to  consider  a  monocular  camera  as  a  bearing-only  measurement  device  and 
use  the  camera  to  estimate  both  TTC  and  bearing.  We  use  an  extended 
Kalman  Filter  (EKF)  to  extract  TTC  from  bearing  measurements. 

The  key  idea  presented  in  this  paper  is  to  maneuver  the  MAV  to  minimize 
the  state  estimation  uncertainty  while  simultaneously  avoiding  obstacles.  We 
will  show  that  these  two  tasks  are  complementary.  We  use  the  local  mapping 
technique  in  our  previous  work  [8,  9,  10],  which  builds  a  polar  map  in  the 
local- level  frame  of  the  MAV  using  the  camera  measurements  directly  without 
transforming  to  the  inertial  frame.  However,  instead  of  using  both  TTC  and 
bearing  measurements  as  in  [8,  9,  10],  in  this  work  we  only  use  bearing 
measurements  to  estimate  both  the  TTC  and  bearing  to  obstacles.  For  this 
purpose  we  will  use  the  nonlinear  observability  theory  developed  by  Hermann 
and  Krener  [11], 

Observability  is  a  measure  of  information  available  for  state  estimation. 
Song  et  al.  [12]  show  that  the  EKF  is  a  quasi-local  asymptotic  observer  for 
discrete-time  nonlinear  systems,  and  that  the  convergence  and  boundedness 
of  the  filter  are  achieved  when  the  system  satisfies  the  nonlinear  observability 
rank  condition  and  when  the  states  stay  within  a  convex  compact  domain. 
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Observability  analysis  has  been  studied  extensively  for  the  purpose  of  estima¬ 
tion  [13,  14,  15].  While  Bryson  and  Sukkarieh  [16]  perform  the  observability 
analysis  of  SLAM  and  develop  an  active  control  algorithm,  the  observability 
analysis  is  not  used  to  develop  active  control.  The  contribution  of  this  paper 
is  that  we  use  the  observability  analysis  to  explicitly  design  the  path  planning 
algorithm.  The  initial  results  of  the  planning  algorithm  are  presented  in  [17]. 
This  paper  extends  the  initial  results  by  conducting  analysis  of  the  collision 
avoidance  and  goal  reaching  behaviors  of  the  planning  algorithm.  The  main 
contributions  of  this  paper  are  as  follows: 

•  We  build  polar  maps  using  the  TTC,  which  are  independent  of  the 
ground  or  air  speed  of  the  MAV. 

•  We  perform  an  observability  analysis  of  the  state  estimation  process 
from  bearing-only  measurements  and  find  the  necessary  conditions  for 
observability  of  the  system. 

•  We  design  a  path  planning  algorithm  based  only  on  the  local  map 
around  the  MAV  in  the  local- level  frame. 

•  The  algorithm  minimizes  the  uncertainties  in  the  TTC  and  bearing 
estimates  while  simultaneously  avoiding  obstacles. 

•  We  analyze  the  behavior  of  the  path  planning  algorithm  and  determine 
the  class  of  environments  where  the  algorithm  guarantees  collision-free 
paths  that  maneuver  the  MAV  to  a  goal  configuration. 

The  paper  is  organized  as  follows.  Section  2  describes  the  model  of  the 
vehicle  in  the  local-level  frame  and  details  a  nonlinear  observability  analysis. 
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In  Section  3  we  describe  the  observability-based  path  planning  algorithm. 
Section  4  analyzes  the  behavior  of  the  path  planning  algorithm.  Numerical 
results  are  provided  in  Section  5,  and  our  conclusions  are  in  Section  6. 

2.  Observability  analysis  of  state  estimation 

In  this  section  we  will  build  a  local  map  using  the  TTC  to  obstacles  in  the 
local-level  frame  of  the  MAV.  The  map  is  constructed  in  polar  coordinates  by 
estimating  the  TTC  and  bearing  to  obstacles.  We  perform  a  nonlinear  ob¬ 
servability  analysis  of  the  state  estimation  problem  using  bearing-only  mea¬ 
surements,  find  necessary  conditions  for  complete  observability  of  the  system, 
and  establish  a  link  between  estimation  accuracy  and  collision  avoidance. 

We  assume  the  MAV  is  flying  at  a  constant  height  above  ground  level  and 
assume  obstacles  in  the  environment  are  static.  Since  the  obstacle  map  is  in 
the  local-level  frame  of  the  MAV,  the  equations  of  motion  of  each  obstacle 
relative  to  the  MAV  need  to  be  derived.  The  origin  of  the  local- level  frame 
is  the  MAV’s  center  of  mass.  The  .x-axis  points  out  the  nose  of  the  airframe 
when  the  airframe  is  not  pitching,  the  y-axis  points  out  the  right  wing  when 
the  airframe  is  not  rolling,  and  the  2-axis  points  into  the  Earth.  Throughout 
the  paper,  we  will  assume  the  zero  wind  conditions.  Let  V  represent  the 
ground  speed  of  the  MAV,  which  is  assumed  to  be  available,  and  let  <p  and 
ip  represent  the  roll  and  heading  angles,  respectively.  Figure  1  shows  the 
motion  of  the  ith  obstacle  relative  to  the  MAV  in  the  local-level  frame,  where 
t1  is  the  TTC,  r/'  is  the  bearing  whose  positive  direction  is  defined  as  the 
right-handed  rotation  about  the  2-axis  of  the  local-level  frame,  and  Ol  is  the 
ith  obstacle.  Based  on  Fig.  1,  the  equations  of  motion  of  the  obstacle  relative 
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Figure  1:  This  figure  shows  the  motion  of  the  ith  obstacle  relative  to  the  MAV.  The  TTC 
and  bearing  to  the  obstacle  are  represented  by  rl  and  rf .  The  ground  speed  is  represented 


by  V.  The  heading  angle  is  represented  by  xjj.  The  ith  obstacle  is  represented  by  0\ 
to  the  MAV  in  terms  of  TTC  and  bearing  are  given  by 


(1) 

(2) 


—  COS  77  , 

sin  T]1 


where,  assuming  coordinated  turn  conditions,  ip  =  (7  tan  <p  and  where  (p  is 


the  roll  angle  of  the  MAV,  which  we  assume  to  be  a  control  signal.  Since 
we  use  the  camera  to  measure  the  bearing  only  (which  only  requires  data 
association),  the  measurement  at  time  t  is  given  by 


(3) 


where  v\  is  the  measurement  noise  that  is  assumed  to  be  a  zero-mean  Gaus¬ 
sian  random  variable.  Based  on  the  state  transition  model  expressed  by 
Eqs.  (1)  and  (2)  and  the  observation  model  expressed  by  Eq.  (3),  we  use  the 
EKF  to  estimate  the  TTC  and  bearing  and  we  build  a  TTC  map  in  the  local- 
level  frame  using  polar  coordinates,  as  shown  in  Fig.  2.  The  origin  of  the 
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map  is  the  current  location  of  the  MAV.  The  circles  represent  the  obstacles 
and  the  ellipses  around  them  represent  the  TTC  and  bearing  uncertainties. 
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Figure  2:  This  figure  shows  the  TTC  map  in  the  local-level  frame  of  the  MAV  using  polar 
coordinates.  The  origin  of  the  map  is  the  current  location  of  the  MAV.  The  circles  represent 
the  obstacles  and  the  ellipses  around  them  represent  the  TTC  and  bearing  uncertainties. 
The  radial  direction  is  TTC  in  units  of  seconds. 

To  decrease  the  uncertainties  in  the  TTC  and  bearing  estimates,  we 
analyze  the  observability  of  the  system  given  by  Eqs.  (1),  (2),  and  (3). 

Let  x*  =  [t1,  rf}J  represent  the  state  vector  associated  with  the  ith  obsta¬ 
cle  and  let  u  =  <fi  represent  the  control  input.  Let  x'  =  f  (x',  u)  represent 
the  state  transition  model  given  by  Eqs.  (1)  and  (2)  and  let  z\  =  h(x\) 
represent  the  observation  model  given  by  Eq  (3).  The  observability  matrix 
is  computed  using  Lie  derivatives  described  by  Hermann  and  Kerner  [11], 
The  0th  order  Lie  derivative  is  ( h)  =  if  and  the  1st  order  Lie  derivative 
is  L\{h)  =  f  =  —ff  -\-  1.  Wc  define  the  vector  of  Lie  derivatives 
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Cl  =  [Lf(h),L}(h)]T.  The  observability  matrix  is  computed  as 


Ch  = 


<9x': 


0  1 

sin?7z  cosr f 

(Tz)2  Ti 


(4) 


The  observability  matrix  has  rank  two  if  and  only  if  r*  ^  oo,  rf  ^  2wp  where 
p  £  Z.  The  EKF  is  a  quasi-local  asymptotic  observer  for  nonlinear  systems 
and  its  convergence  and  boundedness  are  achieved  when  the  system  is  fully 
observable  [12].  Rounds  on  the  EKF  error  covariance  P'  are  related  to  the 
observability  of  the  system  given  by  Lemma  1  proved  in  [12]. 


Lemma  1  (  [12]).  Suppose  that  there  exist  positive  real  scalars  aq,  «2,  fa,  fa 
such  that  fal  <  0*T0*  <  /^I  and  «il  >  C*C*T  >  o^I  then , 


I  <  P*  < 


(5) 


where  I  is  the  identity  matrix  and  C*  is  the  controllability  matrix. 


From  Lemma  1,  we  can  see  that  both  the  maximum  and  minimum  sin¬ 
gular  values  f3\  and  fa  of  the  observability  matrix  should  be  maximized  in 
order  to  minimize  both  the  upper  and  lower  bounds  of  the  error  covariance 
matrix.  For  the  problem  in  this  paper  the  order  of  the  system  is  two,  and 
therefore  minimizing  the  inverse  of  the  determinant  of  O'  O'  will  maximize 
the  two  eigenvalues  of  O'  O'  .  The  determinant  of  O'  O'  related  to  the  ith 
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obstacle  is  given  by 


det(0*T0*)  =  (6) 

From  Eq.  (6),  the  inverse  of  determinant  is  given  by  4^?.  It  can  be 
seen  that  for  large  r®,  the  inverse  is  high,  which  means  observability  is  less, 
because  the  change  in  the  bearing  measurement  is  very  small  with  the  large 
TTC  (low  parallax).  It  can  also  be  seen  that  the  inverse  is  minimum  at 
rf  =  7t/2  and  is  maximum  at  rf  =  0,  which  means  that  the  vehicle  is  moving 
directly  toward  the  obstacle.  Minimizing  the  inverse  will  ensure  that  rf  7^  2pn 
and  will  regulate  rf  — y  7t/2.  This  implies  that  the  minimization  of  the  inverse 
of  the  determinant  will  minimize  the  lower  and  upper  bounds  of  the  error 
covariance  matrix  as  well  as  steer  the  MAV  away  from  the  obstacle.  Therefore 
the  minimization  of  uncertainty  and  obstacle  avoidance  are  complementary. 

3.  Observability-based  path  planning 

Based  on  the  observability  analysis  in  the  previous  section,  we  design 
the  observability-based  path  planning  algorithm  denoted  by  n°  such  that  (a) 
the  uncertainties  in  the  TTC  and  bearing  estimates  are  minimized  and  (b) 
the  MAV  is  maneuvered  to  the  goal  configuration.  For  the  objective  of  goal 
reaching,  the  MAV  requires  knowledge  of  its  own  inertial  position  and  the 
inertial  position  of  the  goal.  Accordingly,  the  path  planning  algorithm  tt° 
requires  the  use  of  GPS. 

Let  rf  and  rft  represent  the  TTC  and  bearing  to  the  goal  configuration  at 
time  t,  and  let  x(y  =  [rf ,  rf(]J .  Let  t]  and  rf  represent  the  estimated  TTC  and 
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bearing  to  the  ith  obstacle  and  let  xj  =  [rt*,^]T.  Let  xt  =  [xjT,  •  •  •  ,  x™T]T. 
The  determinant  of  the  matrix  O'  O'  associated  with  the  ith  obstacle  is  given 
by  det(OjTOj)  =  .  Let  ut  =  [xfT,x4T]T.  Let  Zt  represent  the  index  set 

of  all  n  obstacles  and  let  rl  represent  the  maximum  TTC  to  obstacles  that 
the  planning  algorithm  ir°  reacts  to.  Let  Bt  =  {i  E  Tt  :  t\  <  t1,  \4\  <  f } 
represent  the  index  set  of  obstacles  with  the  TTC  no  greater  than  rl  and  with 
the  azimuth  no  greater  than  |.  Define  the  utility  function  S  :  R2n+2  -4las 

S(vt)  =  a^9)2  +  a2(Vt)2  +  (7) 

sin  m 

i=i  IL 

where  a\,  a2,  bt ,  i  =  1,  •  •  •  ,n  are  non-negative  weights,  and  IBt (i)  is  the 
indicator  function  of  the  index  i,  which  zeros  out  the  contribution  of  obstacles 
that  are  far  away  or  that  are  passed  by  the  MAV. 

By  minimizing  the  first  two  terms  of  Eq.  (7),  the  algorithm  drives  the 
MAV  towards  the  goal  configuration.  The  third  term  penalizes  the  weighted 
sum  of  the  inverse  of  the  determinant  of  O'  O'  for  all  obstacles.  By  minimiz¬ 
ing  this  term,  the  algorithm  achieves  two  objectives  simultaneously.  First, 
it  minimizes  the  uncertainties  in  the  TTC  and  bearing  estimates.  Second, 
the  MAV  is  steered  around  the  obstacles.  It  is  important  to  note  that  these 
two  objectives  are  complementary  to  each  other.  We  use  a  look-ahead  policy 
over  the  horizon  T  to  design  the  path  planner  tt°.  The  cost  function  to  be 
minimized  is  given  by 

rt-\-T 

J  =  Jt  s(up)dP,  (8) 
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subject  to  the  constraints 


X*  =  f(x^,Mp), 

Xp  =  f  (Xp,  Up),  i  =  1,  -  ■  •  ,n,  (9) 

|  ^p  |  f;  0max  • 

To  solve  the  constrained  optimization  problem,  we  discretize  the  time  horizon 
T  as  the  m-step  look-ahead  horizon  {t,t  +  At,  •  •  •  ,t  +  mAt},  where  At  = 
T/m.  Equation  (8)  then  becomes 

m 

j  =  (10> 

j= i 

The  optimal  path  over  the  m-step  look-ahead  horizon  is  found  using  the 
nonlinear  optimization  function  fmincon  in  MATLAB  [18]  and  is  replanned 
once  the  MAV  has  followed  the  first  portion  of  the  m-step  look-ahead  path. 

4.  Analysis 

The  utility  function  given  by  Eq.  (7)  can  be  decomposed  as  the  sum  of 
<Si(xf)  =  a1(rf)2  T  a2(^f)2,  (11) 


and 


S2(xt)  =  ^2biIBt(i) 

2=1 


(riY  t 

sin2  rft 


(12) 
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Accordingly,  the  observability-based  path  planner  tt°  that  minimizes  Eq.  (8) 
can  be  decomposed  into  the  goal  reaching  planner  denoted  by  tt9  that  ma¬ 
neuvers  the  MAV  to  the  goal  by  minimizing  the  cost  function 

rt-\-T 

J\  =  J  Si(x.9p)dp,  (13) 

and  the  collision  avoidance  planner  denoted  by  nc  that  maximizes  the  ob¬ 
servability  of  the  system  by  minimizing  the  cost  function 

rt-\-T 

h  =  J  S2(xp)dp.  (14) 

Remark  1.  We  decompose  the  observability-based  path  planner  n°  into  the 
collision  avoidance  planner  irc  and  the  goal  reaching  planner  ir9  to  simplify 
the  analysis  of  collision  avoidance  and  goal  reaching  behaviors. 

Accordingly,  we  analyze  the  obstacle  avoidance  behavior  of  the  collision 
avoidance  planner  nc  that  maximizes  the  observability  of  the  system  and  de¬ 
scribe  under  what  environment  7 rc  guarantees  collision- free  paths.  We  also 
describe  under  what  environment  the  collision  avoidance  planner  7rc  is  guar¬ 
anteed  to  drive  the  MAV  to  the  goal  when  it  is  combined  with  the  goal 
reaching  planner  ir9. 

4-1.  Collision  avoidance 

We  analyze  the  behavior  of  the  collision  avoidance  planner  7rc  for  avoiding 
circular  obstacles.  The  collision  avoidance  planner  nc  minimizes  the  cost 
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function  given  by  (14)  subject  to  constraints 


i  =  l,---  >ra,  (15) 

Vrlp  >  r\i  =  l,---  ,n,  (16) 

where  r'  is  the  radius  of  the  ith  obstacle.  To  guarantee  to  avoid  a  single 
circular  obstacle,  it  is  necessary  to  establish  a  minimum  turn  away  distance 
dmin  from  the  obstacle.  Let  (jimax  represent  the  maximum  roll  angle  of  the 
MAV  and  let  g  represent  the  gravity  constant.  The  minimum  turning  radius 
is  then  given  by  [19] 


Ant 


l/2 

g  tan(0max) 


(17) 


Theorem  1  shows  the  minimum  turn  away  distance  required  to  avoid  a  cir¬ 
cular  obstacle  with  the  radius  r  using  the  collision  avoidance  planner  7rc. 

Theorem  1.  Using  the  collision  avoidance  planner  7 rc  that  minimizes  the 
cost  function  (14)  subject  to  the  constraints  (15)  and  (16),  collision  avoidance 
with  a  circular  obstacle  with  the  radius  r  is  guaranteed  if  the  initial  condition 
satisfies 


>  dmin  ^ (v  +  A nt)^  Ant  A  (IS) 

where  r0  represents  the  initial  TTC  to  the  circular  obstacle.  In  addition,  the 
MAV  converges  to  a  circle  around  the  obstacle  with  the  radius  max{r,  rmt}. 

Proof.  Consider  the  worst  case  scenario  where  the  MAV  is  flying  directly 
toward  a  circular  obstacle  Ol  in  the  local-level  frame  map,  as  shown  in  Fig.  3. 
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The  minimum  turn  away  distance  dmin  from  the  obstacle  can  be  determined 
when  the  maximum  roll  angle  </>max  is  applied  and  the  generated  circle  with 
the  minimum  turning  radius  rmt  is  tangent  to  the  surface  of  the  obstacle. 
Based  on  the  geometry,  the  planner  nc  is  guaranteed  to  avoid  the  obstacle  if 
the  initial  condition  satisfies  Vto  >  dm |n  =  y/(r  +  rmt)2  —  r^t  —  r. 

To  show  that  the  trajectory  converges  to  an  orbit  around  the  obstacle,  if 
Vtq  >  dmin,  the  collision  avoidance  planner  7 rc  will  cause  the  MAV  to  move 
in  such  a  way  that  the  TTC  to  the  obstacle  decreases  and  the  bearing  to  the 
obstacle  increases.  The  MAV  will  first  reach  a  configuration  at  time  t  where 
the  bearing  to  the  obstacle  rjt  =  §  and  the  range  to  the  obstacle  Vrt  >  r. 
Then  the  planner  nc  will  further  cause  the  MAV  to  reach  a  configuration  at 
time  t'  such  that  77*/  =  |  and  Vrt  >  Vt#  >  r.  This  process  is  repeated  such 
that  the  TTC  decreases  progressively.  Because  of  the  constraints  (16)  on 
the  TTC  and  the  minimum  turning  radius  constraint,  the  MAV  converges  to 
max{  r,rmt}.  □ 


Figure  3:  This  figure  shows  the  worst  case  scenario  that  the  MAV  is  flying  directly  toward 
a  circular  obstacle. 

To  this  point  we  have  found  the  conditions  under  which  a  single  circu- 
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lar  obstacle  can  be  successfully  avoided  using  7rc.  We  extend  the  analysis 
to  investigate  the  multiple  obstacle  avoidance  problem.  Our  approach  char¬ 
acterizes  the  environment  with  minimum  separation  between  obstacles  such 
that  collisions  are  avoided  with  all  the  obstacles. 

Let  C  represent  the  configuration  space.  For  two  configurations  q\  = 
[qin,  qie,  qiiiiV  e  C  and  q2  =  [<?2nW2e,«?2y]T  e  C,  where  qin  and  qie,  i  =  1,2, 
represent  North  and  East  coordinates,  and  q^,i  =  1,2,  represent  the  heading 
angle,  define  the  distance  between  qi  and  q2  as 

Ikl  -  92  1 1  =  V  ( qvn  -  <?2n)2  +  (<7le  ~  <?2e)2-  (19) 

For  a  configuration  q  and  the  ith  obstacle  0\  we  define  the  distance  between 
q  and  the  boundary  of  O'  as 


dl~  min  ||g-g'||.  (20) 

q'EdO1 

Let  dlmin  and  (II,nili  represent  the  minimum  turn  away  distance  for  the  ith 
obstacle  O'  and  the  jth  obstacles  0J  given  by  Eq.  (18).  Let 


dij 


min 

PiedO\pjedOd 


hi  ii 


(21) 


represent  the  shortest  distance  between  the  points  along  the  boundaries  of 
O'  and  CP .  Let  (jo  represent  the  initial  MAV  configuration.  Theorem  2  de¬ 
scribes  the  characteristics  of  the  environment  in  which  the  collision  avoidance 
planner  nc  guarantees  collision-free  paths. 

Theorem  2.  If  the  environment  satisfies  oO  >  rnax{d'ni„,  Vi,j  and 
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the  initial  MAV  configuration  q0  satisfies  dqo  >  dlmin ,  Mi,  where  dli  is  the 
distance  between  the  ith  and  the  jth  obstacles  given  by  Eq.  (21),  dlmin  and  dfin 
represent  the  minimum  turn  away  distance  for  the  ith  and  the  jth  obstacles 
given  by  Eq.  (18),  and  dqo  is  the  distance  between  q0  and  the  ith  obstacle 
given  by  Eq.  (20),  then  the  collision  avoidance  planner  irc,  which  minimizes 
the  cost  function  (14)  subject  to  constraints  (15)  and  (16),  guarantees  that 
the  MAV  will  avoid  all  obstacles  in  the  future. 

Proof.  Consider  that  the  MAV  is  initially  located  at  go  with  dqo  >  d‘min ,  Mi, 
and  that  it  will  collide  with  an  obstacle  Ol  if  it  flies  along  its  initial  heading, 
as  shown  in  Fig.  4.  Since  dlqo  >  dlmin  and  dl]  >  max{cljrijn,  d3min } ,  in  the  worse 
case  scenario  the  planner  7rc  leads  to  a  collision-free  path  from  g0  to  (Ja  on  the 
boundary  of  Ol  with  direction  tangent  to  the  boundary,  where  d?q  >  djnin . 
This  means  that  the  MAV  certainly  has  the  capability  to  avoid  the  obstacle 
Oj  when  it  reaches  q\-  In  addition,  since  djk  >  max{<jVn,  d(nin}.  in  the  worse 
case  scenario  the  planner  nc  leads  to  a  collision-free  path  from  <p\  to  (Jb  on  the 
boundary  of  0J  with  direction  tangent  to  the  boundary,  where  dqB  >  dflin . 
This  process  can  be  repeated  so  that  the  MAV  does  not  collide  with  any 
obstacle  using  7rc  for  all  time  t.  □ 

4-2.  Goal  reaching 

Besides  the  collision  avoidance  behavior  of  the  planner  nc,  we  are  also 
interested  in  its  goal  reaching  behavior  when  it  is  combined  with  the  planner 
7i9.  In  this  section,  we  combine  the  two  path  planners  using  a  switching 
algorithm  that  executes  them  alternately. 
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Figure  4:  This  figure  shows  the  collision  avoidance  planner  7rc  maneuvers  the  MAV  to 
avoid  multiple  obstacles. 

Remark  2.  We  analyze  the  goal  reaching  behavior  of  the  switching  algorithm 
to  simplify  the  determination  of  analytical  conditions  under  which  the  colli¬ 
sion  avoidance  planner  irc  is  guaranteed  to  drive  the  MAV  to  the  goal  when 
it  is  combined  with  the  goal  reaching  planner  tt9  .  However,  in  simulation 
we  use  the  observability-based  planning  algorithm  n°  that  takes  into  account 
collision  avoidance  and  goal  reaching  simultaneously. 

The  switching  algorithm  is  described  as  follows.  The  algorithm  first  ex¬ 
ecutes  the  goal  reaching  planner  tt9  to  maneuver  the  MAV  toward  the  goal 
from  an  initial  configuration.  If  there  exist  obstacles  with  the  TTC  no  greater 
than  t1  that  collide  with  the  MAV,  the  algorithm  executes  the  collision  avoid¬ 
ance  planner  7 rc  to  react  to  nearby  obstacles.  The  collision  avoidance  plan¬ 
ner  7rc  is  executed  until  the  MAV  reaches  a  configuration  such  that  the  goal 
reaching  planner  n9  can  generate  a  path  from  that  configuration  to  the  goal, 
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which  does  not  collide  with  any  obstacle  with  TTC  less  than  rl .  The  algo¬ 
rithm  then  executes  the  goal  reaching  planner  7 r9.  This  process  is  repeated 
until  the  MAV  reaches  the  goal. 

The  switching  algorithm  needs  conditions  on  the  environment  to  ensure 
that  it  drives  the  MAV  to  the  goal.  The  following  theorem  describes  the  con¬ 
ditions  under  which  the  MAV  can  reach  the  goal  from  an  initial  configuration 
with  the  heading  pointing  to  the  goal  using  the  switching  algorithm. 

Theorem  3.  If  the  environment  satisfies  d 19  >  2 Vt1,  Vi ,  j  and  the  initial 
MAV  configuration  qo  with  the  heading  pointing  to  the  goal  satisfies  dlqo  > 
dlmin,  Vi,  then  the  MAV  is  guaranteed  to  be  maneuvered  to  the  goal  using  the 
switching  algorithm. 

Proof.  Let  qo  represent  the  initial  MAV  configuration  with  the  heading  point¬ 
ing  to  the  goal  at  time  t0  as  shown  in  Fig.  5  and  let  dto  =  1 1 qo  —  <jf  \ |  represent 
the  distance  between  q0  and  qf.  Consider  a  scenario  that  there  exists  an 
obstacle  Ol  in  the  MAV’s  initial  course  towards  the  the  goal  qf  and  that 
the  TTC  to  the  obstacle  is  no  greater  than  rl.  For  this  scenario,  since 
dP  >  2 Vt1,  Vi,j,  there  are  no  other  obstacles  with  TTC  no  greater  than 
t1  when  the  MAV  is  located  at  qo.  The  switching  algorithm  executes  the  col¬ 
lision  avoidance  planner  7rc  to  react  to  the  obstacle  Ol.  Based  on  Theorem  1, 
the  planner  7rc  will  cause  the  distance  between  the  MAV  and  the  obstacle  O' 
to  decrease  until  the  MAV  converges  to  a  circle  with  the  radius  max{r,rmt}. 
This  implies  that  the  MAV  will  stay  within  the  circle  Cl  centered  at  Ol  with 
the  radius  Vt1,  where  the  planner  tV  only  reacts  to  the  obstacle  O'  until 
the  MAV  converges  to  the  circle  with  the  radius  max{r,rmt}.  Accordingly, 
the  switching  algorithm  executes  the  collision  avoidance  planner  7rc  until  the 
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MAV  reaches  a  configuration  qi  at  time  t\  such  that  the  goal  reaching  planner 
7i9  can  generate  a  path  from  (/i  to  qj  that  does  not  collide  with  the  obstacles 
0\  While  the  MAV  flies  from  q0  to  q\  using  7 rc,  the  bearing  to  the  obstacle  O' 
is  no  greater  than  | .  Since  the  MAV  inertial  angle  to  the  goal  during  its  flight 
from  q0  to  q\  is  less  than  the  bearing  to  the  obstacle,  the  inertial  angle  to  the 
goal  must  be  less  than  |.  Therefore,  it  must  be  that  dtl  =  \\qi  —  qq\\  <  dto. 

Once  the  MAV  reaches  q±,  the  algorithm  executes  the  goal  reaching  plan¬ 
ner  77 g  until  the  MAV  reaches  a  configuration  q2  outside  of  the  circle  Cl , 
where  another  obstacle  0:)  with  TTC  no  greater  than  t1  exists  in  the  MAV’s 
course  towards  the  goal.  It  is  apparent  that  dt2  =  \\q2  —  qq ||  <  dtl.  Once 
the  MAV  reaches  q2,  the  switching  algorithm  executes  the  collision  avoidance 
planner  7rc  to  react  to  the  obstacle  OK  As  the  process  is  repeated,  the  dis¬ 
tance  between  the  MAV  and  the  goal  decreases  progressively  and  the  MAV 
will  be  eventually  maneuvered  to  the  goal  using  the  switching  algorithm.  □ 

The  conditions  given  by  Theorem  3  require  that  each  two  obstacles  in 
the  environment  are  separated  far  enough  so  that  the  MAV  reacts  to  and 
avoids  obstacles  one  by  one  until  it  reaches  the  goal.  We  assume  the  envi¬ 
ronment  satisfies  these  conditions  in  order  to  provide  a  theoretical  guarantee 
for  the  goal  reaching  behavior  of  the  collision  avoidance  planner  nc  when  it 
is  combined  with  the  goal  reaching  planner  n9.  The  conditions  may  not  be 
necessary  for  the  observability-based  planning  algorithm  7r°  to  achieve  goal 
reaching  performance.  This  implies  that  there  may  exist  environments  that 
do  not  satisfy  the  conditions  but  where  the  MAV  can  still  be  maneuvered  to 
the  goal  without  causing  collisions  using  n°. 
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Figure  5:  This  figure  shows  the  MAV  can  be  maneuvered  to  qf  using  the  switching  algo¬ 
rithm. 

5.  Numerical  results 

In  this  section,  we  tested  the  observability-based  planning  algorithm  7T° 
that  minimizes  the  cost  function  (8)  and  takes  into  account  collision  avoid¬ 
ance  and  goal  reaching  simultaneously  using  a  simulation  environment  devel¬ 
oped  in  MATLAB/SIMULINK.  The  simulator  uses  a  six  degree  of  freedom 
model  of  the  aircraft.  The  coordinate  system  is  represented  by  NED  (North- 
East-Down)  system.  The  covariance  matrices  of  the  process  and  measure- 

(  0.001  0  \ 

ment  noises  were  Q*  =  I  and  R*  =  0.0012.  The  weighting 

y  0  0.0076  j 

scalars  op  and  op  were  10  and  1.  All  the  weighting  scalars  6*  =  2,  i  =  1,  •  •  •  ,  n. 
A  look-ahead  policy  over  a  horizon  3.6  seconds  was  used.  The  ground  speed 
was  V  =  13  m/s.  The  maximum  roll  angle  for  the  MAV  was  30°.  We  tested 
the  algorithm  for  both  single  and  multiple  obstacle  avoidance  scenarios.  We 
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also  conducted  Monte  Carlo  simulations  to  test  the  collision  avoidance  and 
goal  reaching  performance  of  the  observability-based  planning  algorithm  n° 
with  varying  measurement  uncertainties  in  the  environments  with  varying 
minimum  distance  between  obstacles. 

5.1.  Single  obstacle  avoidance 

In  this  scenario,  the  MAV  was  commanded  to  maneuver  around  an  ob¬ 
stacle  located  at  (150,250)  between  waypoint  S  (0,100,-40)  and  waypoint  E 
(600,700,-40)  represented  by  the  box  and  plus  signs  shown  in  Fig.  6(a). 

Figure  6  shows  the  path  followed  by  the  MAV  for  avoiding  the  obstacle 
using  the  planning  algorithm  7r°,  the  determinant  of  O'  O'  for  that  obstacle, 
the  TTC  and  bearing,  and  the  TTC  and  bearing  estimation  error.  It  can 
be  seen  that  when  the  determinant  is  maximum,  then  the  bearing  is  p  =  | 
and  the  TTC  reaches  its  minimum  value  rmin  ~  4  s.  At  the  same  time,  the 
bound  on  the  error  covariance  for  the  TTC  is  minimum,  which  shows  that 
the  uncertainties  in  state  estimates  can  be  minimized  while  simultaneously 
avoiding  collisions. 

5.2.  Multiple  obstacle  avoidance 

In  the  multiple  obstacle  avoidance  scenario,  the  MAV  was  commanded  to 
maneuver  through  twenty-five  obstacles  between  waypoint  S  (0,100,-40)  and 
waypoint  E  (600,700,-40),  as  shown  in  the  subfigures  on  the  right  of  Fig.  7. 

Figure  7  shows  the  evolution  of  the  local  map  in  the  local- level  frame 
and  the  update  of  the  path  in  the  inertial  frame  at  different  time.  The 
dashed  circles  in  the  subfigures  on  the  left  represent  the  TTC  at  3  s,  6  s, 
and  9  s  for  the  inner,  middle,  and  outer  circles  respectively.  The  plus  sign  in 
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0.01 


(a)  Inertial  path 


(c)  TTC  (d)  Bearing 


(e)  TTC  error  (f)  Bearing  error 


Figure  6:  This  figure  shows  the  simulation  results  for  single  obstacle  avoidance  problem. 
Subfigure  (a)  shows  the  inertial  path.  Subfigure  (b)  shows  the  determinant  of  CFOh 
Subfigures  (c)  and  (d)  show  the  TTC  and  bearing  to  the  obstacle.  Subfigure  (e)  and 
(f)  show  the  estimation  error  and  ±3 a  bounds  of  the  error  covariance  for  the  TTC  and 
bearing. 
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Right  wing  direction  (m) 


(a)  t=20  s 


Right  wing  direction  (m) 


(b)  t=35  s 


Right  wing  direction  (m) 


(c)  t=53  s 


Right  wing  direction  (m) 


(d)  t=65  s 

Figure  7:  This  figure  shows  the  evolution  of  the  local  map  and  the  update  of  the  path  at 
different  times.  Subfigures  on  the  left  show  the  evolution  of  the  local  map.  The  dashed 
circles  represent  the  TTC  at  3  s,  6  s,  and  9  s  for  inner,  middle,  and  outer  circles  respectively. 
Subfigures  on  the  right  show  the  path  in  the  inertial  frame.  The  black  lines  represent  the 
three-step  look-ahead  paths  and  red  lines  represent  the  actual  path  followed  by  the  MAY. 
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subfigure  (d)  on  the  left  represents  the  waypoint  E  in  the  local-level  frame. 
Red  lines  in  the  subfigures  on  the  right  represent  the  paths  followed  by  the 
MAV  and  black  fines  represent  the  optimal  look-ahead  paths.  Figure  8  shows 
the  TTC  and  bearing  to  the  obstacle  located  at  (150,250),  the  TTC  and 
bearing  estimation  error,  and  the  determinant  of  O'  O*  for  that  obstacle. 
We  can  see  that  minimizing  the  cost  function  for  multiple  obstacle  avoidance 
gives  the  same  behavior  for  the  obstacle  avoidance,  observability  and  further 
estimation  uncertainties. 

Figure  9  shows  how  the  value  of  the  cost  function  changes  as  time  pro¬ 
gresses.  Based  on  the  figure,  the  cost  function  decreases  initially  when  there 
are  no  obstacles  in  the  local  map.  The  cost  function  only  consists  of  the  first 
term.  Once  a  new  obstacle  pops  up,  the  cost  function  increases  because  the 
obstacle  term  is  added  to  the  cost  function.  The  planning  algorithm  tt°  then 
minimizes  the  second  term,  causing  the  cost  function  to  decrease.  Once  the 
collision  is  avoided  and  the  obstacle  is  passed,  it  does  not  add  any  cost  to 
the  cost  function.  The  cost  function  then  decreases  based  on  the  first  term. 
Similar  behavior  occurs  when  multiple  obstacles  are  observed. 

5.3.  Monte  Carlo  simulation 

To  simplify  the  analysis  of  collision  avoidance  and  goal  reaching  perfor¬ 
mance  and  determine  analytical  conditions,  in  previous  section  we  decompose 
the  observability-based  planning  algorithm  tt°  into  the  collision  avoidance 
planner  irc  and  the  goal  reaching  planner  tt9 .  We  then  analyze  of  obsta¬ 
cle  avoidance  behavior  of  the  planner  7rc  and  the  goal  reaching  behavior  of 
the  switching  algorithm  that  executes  the  two  planners  alternately.  Accord¬ 
ingly,  the  conditions  for  collision  avoidance  and  goal  reaching  described  in 
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(a)  TTC  (b)  Bearing 


(c)  TTC  error 


(d)  Bearing  error 


(e)  Determinant 


Figure  8:  This  figure  shows  the  TTC  and  bearing  to  the  obstacle  located  at  (150,250),  the 
TTC  and  bearing  tracking  error  and  the  determinant  of  CF  Ob  Subfigures  (a)  and  (b) 
show  the  TTC  and  bearing.  Subfigure  (c)  and  (d)  show  the  error  and  ±3cr  bounds  of  the 
error  covariance.  Subfigure  (e)  shows  the  determinant  of  Oz  0\ 
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Figure  9:  This  picture  shows  the  change  of  the  value  of  the  cost  function  as  time  progresses. 

Theorem  2  and  3  may  not  be  identical  for  the  observability-based  planner 
7r°  to  achieve  collision  avoidance  and  goal  reaching  performance.  Accord¬ 
ingly,  we  conducted  Monte  Carlo  simulations  to  demonstrate  the  statistical 
performance  of  the  observability-based  planning  algorithm  n°. 

For  each  environment  with  a  fixed  minimum  distance  between  obstacles, 
we  executed  100  simulation  runs.  In  each  simulation  run,  the  MAV  was  ma¬ 
neuvered  from  the  initial  position  (120,120,-40)  to  the  end  position  (580,580,- 
40)  through  an  environment  using  n°.  The  environment  was  constructed  so 
that  each  obstacle  was  added  to  the  environment  based  on  a  uniform  distri¬ 
bution  over  the  square  area  with  the  South-West  corner  (100,100)  and  the 
North-East  corner  (600,600)  until  no  more  obstacles  could  be  added.  The 
height  and  radius  for  all  obstacles  were  100  meters  and  20  meters,  and  the 
MAV  was  flying  at  a  height  of  40  meters.  The  observability-based  planner 
7r°  reacts  to  obstacles  with  TTC  no  greater  than  t1  =4  s.  We  evaluate  two 
criteria:  the  number  of  collisions  and  the  percentage  of  runs  where  MAV 
reached  the  goal.  We  say  that  the  MAV  reaches  the  goal  if  it  is  maneuvered 
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to  the  goal  in  t  <  100  seconds  without  any  collisions. 

Figure  10  (a)  plots  the  average  number  of  collisions  over  100  simulation 
runs  versus  the  minimum  distance  between  obstacles  for  the  case  where  the 
standard  deviation  of  the  bearing  measurement  noise  is  2°,  as  shown  by  the 
solid  line,  and  for  the  case  where  the  locations  of  obstacles  are  perfectly 
known,  as  shown  by  the  dashed  line.  The  figure  shows  the  average  num¬ 
ber  of  collisions  decreases  dramatically  as  the  minimum  distance  between 
obstacles  increases  from  5  to  20  meters  for  both  cases.  After  the  minimum 
distance  is  greater  than  20  meters,  the  average  number  of  collisions  decreases 
slowly  for  the  case  with  measurement  uncertainties  and  the  average  number 
of  collisions  is  zero  for  the  case  where  the  locations  of  obstacles  are  perfectly 
known.  The  results  match  the  obstacle  avoidance  behavior  of  the  collision 
avoidance  planner  7rc.  Given  V  —  13  m/s,  ^>max  =  30°,  and  Ri  =  20  m,  Vi, 
the  minimum  distance  satisfying  the  obstacle  avoidance  conditions  of  The¬ 
orem  2  for  the  planner  ttc  is  19.9345  meters.  When  the  minimum  distance 
is  less  than  19.9345  meters,  the  number  of  collisions  decreases  quickly  as 
the  minimum  distance  increases.  When  the  minimum  distance  is  greater 
than  19.9345  meters,  the  conditions  of  Theorem  2  are  satisfied.  The  collision 
avoidance  planner  irc  guarantees  collision-free  paths  if  the  obstacle  locations 
are  perfectly  known.  For  the  case  with  the  measurement  uncertainties,  the 
MAV  still  encounters  a  small  number  of  collisions  when  the  minimum  dis¬ 
tance  is  greater  than  20  meters.  In  addition,  when  the  minimum  distance  is 
10  and  15  meters  for  the  case  with  perfectly  known  obstacle  locations,  the 
average  number  of  collisions  is  less  than  one,  which  implies  that  there  exist 
environments  that  do  not  satisfy  the  conditions  of  Theorem  2  but  where  the 
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observability-based  planner  tt°  still  generates  collision-free  paths. 

Figure  10  (b)  plots  the  percentage  of  runs  where  the  MAV  reached  the 
goal  versus  the  minimum  distance  for  the  two  cases.  The  percentage  increases 
as  the  minimum  distance  between  obstacles  increases.  When  the  minimum 
distance  is  greater  than  80  meters  for  the  case  with  measurement  uncertain¬ 
ties  or  when  the  minimum  distance  is  greater  than  70  meters  for  the  case 
with  perfectly  known  obstacle  locations,  the  MAV  is  always  maneuvered  to 
the  goal  using  the  observability-based  planner.  In  addition,  the  percentage 
for  all  the  environments  with  the  minimum  distance  from  10  to  100  meters 
for  both  cases  is  nonzero.  Accordingly,  the  minimum  distance  2 Vrl  —  104  m, 
which  satisfies  the  goal  reaching  conditions  of  Theorem  3  for  the  switching 
algorithm,  is  not  necessary  for  the  observability-based  planner  tt°  to  achieve 
goal  reaching  performance. 


Figure  10:  This  figure  shows  the  statistical  performance  of  the  observability-based  plan¬ 
ning  algorithm  implemented  in  the  environments  with  varying  minimum  distance  between 
obstacles  for  the  cases  with  the  standard  deviation  of  the  bearing  measurement  noise  2° 
and  with  perfectly  known  obstacle  locations.  Subfigure  (a)  plots  the  average  number  of 
collisions  over  100  simulation  runs  versus  the  minimum  distance  between  obstacles.  Subfig¬ 
ure  (b)  plots  the  percentage  of  runs  where  the  MAV  reached  the  goal  versus  the  minimum 
distance. 
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Figure  11:  This  figure  shows  the  statistical  performance  of  the  observability-based  planning 
algorithm  with  varying  measurement  uncertainties  in  the  environment  with  the  minimum 
distance  30  meters.  Subfigure  (a)  plots  the  average  number  of  collisions  over  100  simulation 
runs  versus  the  standard  deviation  of  the  bearing  measurement  noise.  Subfigure  (b)  shows 
the  percentage  of  runs  where  the  MAV  reached  the  goal  versus  the  standard  deviation. 

To  take  into  account  the  effect  of  measurement  uncertainties,  Monte  Carlo 
simulations  were  also  conducted  to  test  the  performance  of  the  algorithm  n° 
with  varying  measurement  uncertainties.  Similarly,  we  evaluate  the  number 
of  collisions  and  the  percentage  of  runs  for  the  MAV  to  reach  the  goal.  Fig¬ 
ure  11  plots  the  average  number  of  collisions  over  100  simulation  runs  and 
the  percentage  of  runs  where  the  MAV  reached  the  goal  versus  the  standard 
deviation  of  the  bearing  measurement  noise  for  the  environment  with  the 
minimum  distance  30  meters.  Based  on  the  figure,  as  the  standard  deviation 
of  the  measurement  noise  increases,  the  number  of  collisions  increases  and 
the  percentage  of  runs  where  the  MAV  reached  the  goal  decreases. 

6.  Conclusions 

This  paper  presents  an  observability-based  planning  algorithm  using  bearing- 
only  measurements.  We  perform  a  nonlinear  observability  analysis  for  state 
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estimation  and  argue  that  collision  avoidance  and  uncertainty  minimization 
problems  are  complementary.  Based  on  this  analysis,  we  design  a  cost  func¬ 
tion  that  minimizes  the  estimation  uncertainties  while  simultaneously  avoid¬ 
ing  obstacles.  By  minimizing  the  cost  function,  the  path  planning  algorithm 
is  developed  directly  in  the  local-level  frame.  We  use  a  look-ahead  pol¬ 
icy  to  plan  optimal  paths  over  a  finite  time  horizon.  The  performance  of 
the  planning  algorithm  is  analyzed  and  the  characteristics  of  the  environ¬ 
ments  in  which  the  planning  algorithm  guarantees  collision-free  paths  that 
lead  to  a  goal  configuration  are  described.  Numerical  results  show  that  the 
observability-based  planning  algorithm  is  successful  in  solving  the  single  and 
multiple  obstacle  avoidance  problems  while  improving  the  estimation  accu¬ 
racy. 
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